#include "vtol.h"

sVTOL vtol;

 void VTOL_Init(sVTOL *ele){ 
	ele->vtol_last_mode = Normal_Mode;
	ele->vtol_mode = Normal_Mode;
	ele->taking_off_step = taking_off_start;
	ele->landing_step = landing_start;
	ele->yaw_com_theta = ahrs[1].Ang[2]; 
	ele->tar_pwm = VTOL_Power_Off_PWM;
	ele->tar_height = flow.height.height;
	
	 
	ele->pwm_increasing_spd = 2;
	ele->pwm_decreasing_spd = 4;
	ele->rising_spd = 0.001f;   //0.2m/s 
	ele->falling_spd = 0.001f;  //0.2m/s
	
	ele->take_off_waiting_cnt = 0;
	ele->landing_waiting_cnt = 0;
}


void VTOL_Take_Off_Init(sVTOL *ele){
	ele->vtol_last_mode = ele->vtol_mode;
	ele->vtol_mode = Taking_Off_Mode;
	ele->tar_height = flow.height.height*0.01f;
	ele->taking_off_step = taking_off_start;
}


void VTOL_Land_Init(sVTOL *ele){
	ele->vtol_last_mode = ele->vtol_mode;
	ele->vtol_mode = Landing_Mode;
	ele->tar_height = flow.height.height*0.01f;
	ele->landing_step = landing_start;
}



void Taking_Off_Mode_Step(sVTOL *ele){
	switch(ele->taking_off_step){	
		case taking_off_start:   //0
			ele->yaw_com_theta = ahrs[1].Ang[2];
			ele->tar_pwm = VTOL_Power_Off_PWM;
			ele->take_off_waiting_cnt = 0;
			ele->taking_off_step = taking_off_idling;		
		break;
		
		case taking_off_idling:	//1
			ele->tar_pwm += ele->pwm_increasing_spd;
			if(ele->tar_pwm>=VTOL_IDLING_PWM){
				ele->tar_pwm = VTOL_IDLING_PWM;
				ele->taking_off_step = taking_off_idling_waiting;
			}
		break;
		
		case taking_off_idling_waiting:	//2
			ele->take_off_waiting_cnt++;
			if(ele->take_off_waiting_cnt>=VTOL_WATING_CNT){
				ele->take_off_waiting_cnt = 0;
				ele->taking_off_step = taking_off;
			}
		break;
		
		case taking_off:	//3
			ele->tar_height += ele->rising_spd;
			if(ele->tar_height>=VTOL_HOVER_HEIGHT){
				ele->tar_height = VTOL_HOVER_HEIGHT;
				ele->taking_off_step = taking_off_hover_waiting;
			}
		break;
		
		case taking_off_hover_waiting://4
			ele->take_off_waiting_cnt++;
			if(ele->take_off_waiting_cnt>=VTOL_WATING_CNT
				||fabs(ele->tar_height-flow.height.height*0.01f)<=0.1){
				ele->take_off_waiting_cnt = 0;
				ele->taking_off_step = hovering_waiting;
			}
		break;
		
		case hovering_waiting:	//5
			ele->tar_pwm = VTOL_Power_Off_PWM;
			ele->tar_height = VTOL_HOVER_HEIGHT;
			ele->vtol_last_mode = ele->vtol_mode;
			ele->vtol_mode = Normal_Mode;
		break;
		
		default:break;
	}
	
}


void Landing_Mode_Step(sVTOL *ele){
	switch(ele->landing_step){
		case landing_start:	//0
			ele->yaw_com_theta = ahrs[1].Ang[2];
			ele->landing_waiting_cnt = 0;
			ele->tar_pwm = VTOL_RISING_PWM;
			ele->landing_step = landing_waiting;
		break;
	
		case landing_waiting: //1
			ele->landing_waiting_cnt++;
			if(ele->landing_waiting_cnt >= VTOL_WATING_CNT){
				ele->landing_waiting_cnt = 0;
				ele->landing_step = landing;
			}
		break;
			
		case landing:	//2
			ele->tar_height -= ele->falling_spd;
			if(ele->tar_height<=VTOL_LANDING_HEIGHT){
				ele->tar_height = VTOL_LANDING_HEIGHT;
				ele->landing_step = landing_hover;
			}	
		break;
		
		case landing_hover:	//3
			ele->landing_waiting_cnt++;
			if(ele->landing_waiting_cnt>=VTOL_WATING_CNT){
				ele->landing_waiting_cnt = 0;
				ele->tar_pwm = mot[0].PWM[0];
				ele->landing_step = landing_idling;
			}
		break;
			
		case landing_idling:	//4
			ele->tar_pwm -= ele->pwm_decreasing_spd;
			if(ele->tar_pwm<=VTOL_Power_Off_PWM){
				ele->tar_pwm = VTOL_Power_Off_PWM;
				ele->landing_step = landing_power_off;
			}
		break;
		
//		case landing_waiting:	//5
//			ele->landing_waiting_cnt++;
//			if(ele->landing_waiting_cnt>=VTOL_WATING_CNT){
//				ele->landing_waiting_cnt = 0;
//				ele->tar_pwm = VTOL_Power_Off_PWM;
//				ele->landing_step = landing_power_off;
//			}
//		break;
		
		case landing_power_off:	//5
			ele->tar_pwm = VTOL_Power_Off_PWM;
			ele->tar_height = flow.height.height*0.01f;
		break;
	
		default:break;
	}
	
}
